#ifndef DEFORMABLEOBJ_H
#define DEFORMABLEOBJ_H

#include <math.h>

#include "model_obj.h"
#include "vmatrix.h"
#include "lamatrix.h"
#include "epslon.h"
//#include "clustersubset.h"
#include "DiagMatrix.h"
#include "myclustergraph.h"

class FractureMesh;

class Connection {
public:
	float length;
	std::map< int, bool > vertices;
	//std::map< int, MyClusterGraph > vertices;
	
};

class VertexCluster
{
public:
	//static const float EPSILON;

	VertexCluster( FractureMesh& mesh, int clusterId );

	void addVertex( ModelOBJ::OBJVertex* vertex, ModelOBJ::OBJVertex* speed, ModelOBJ::OBJVertex* intermediatePosition, ModelOBJ::OBJVertex* intermediateSpeed, double weight );
	void replaceVertex( int vertexIndex, ModelOBJ::OBJVertex* vertex, ModelOBJ::OBJVertex* speed, ModelOBJ::OBJVertex* intermediatePosition, ModelOBJ::OBJVertex* intermediateSpeed );

	virtual void initialize();

	void update( float dt );
	void updateSleeping( float dt );
	//void consolidate();

	virtual void load();
	virtual void preLoad();

	virtual void updateInternal( float dt ) = 0;
	virtual void updateQVector( LAMatrix& Sp ) = 0;

	bool isBlocked;

	//PlasticityMode getPlasticityMode() const { return plasticityMode; }
	//void setPlasticityMode( PlasticityMode mode ) { plasticityMode = mode; }

	//void updateCenterOfMass( ModelOBJ::OBJVertex worldPos );
	//float applyCollisionConstraints( float dt );
	//void calculatingIntermediateSpeedAndPosition( float dt );

	void applyPlasticDeformation( float dt );

	//ModelOBJ::OBJVertex& getIntermediatePosition( int i ) 
	//{
	//	return intermediatePosition[ i ];
	//}

	float inline getDetA() const { return detA; }

	void setVertexWeight( int index, double weight );
	double getVertexWeight( int index );
	void onWeightChanged();

	inline int getNumberOfVertices() 
	{
		return numberOfVertices;
	}

	bool getRecalculateWeights() const { return recalculateWeights; }
	void setRecalculateWeights(bool recalculateWeights) { this->recalculateWeights = recalculateWeights; }

	inline void getCenterOfMass( float* x, float* y, float* z )
	{
		*x = cm[0];
		*y = cm[1];
		*z = cm[2];
	}

	inline Connection addConnection( int clusterId, float length )
	{
		connections[ clusterId ] = Connection();
		connections[ clusterId ].length = length;
		connections[ clusterId ].vertices = std::map< int, bool >();
		return connections[ clusterId ];
	}

	inline void removeConnection( int clusterId )
	{
		connections.erase( clusterId );
	}

	inline int getNumberOfConnections()
	{
		return connections.size();
	}

	inline float getConnectionLength( int clusterId )
	{
		return connections[ clusterId ].length;
	}

	std::map<int,Connection>& getConnections()
	{
		return connections;
	}

	//bool blarg;


	inline bool isConnectedTo( int clusterId )
	{
		return connections.find( clusterId ) != connections.end();
	}

	inline float* getColor() {
		return color.rgb;
	}

	inline bool getIsSleeping() {
		return isSleeping;
	}

	inline void awake() {
		isSleeping = false;
	}

	//void recalculateCenterOfMass();
	void calculateCenterOfMass( float* cX, float* cY, float* cZ );

	void shrinkOriginalVertexBuffer();

	static float MAX_A_DET;

protected:

	//static const float RESTITUTION_DISSIPATION;
	static const float C_YIELD;
	static const float C_CREEP;
	static const float C_MAX;
	//static const float MIN_A_DET;
	

	FractureMesh& mesh;

	Color color;

	float getMeshAlpha();
	float getMeshBeta();

	// outer product between two vectors
	void outerProduct( const float* v1, const float* v2, float* mat );

	//LAMatrix outerProduct( const float* v1, int size1, const float* v2, int size2 );
	void outerProduct( const float* v1, int size1, const float* v2, int size2, LAMatrix& mat, float weight );

	bool calculateRMatrix();
	void calculateAMatrix();

	void updateSpeedAndPosition( const float* relativeGi, int vertexIndex, float dt );
	
	LAMatrix Sp;
	VMatrix matR;
	VMatrix matA;
	VMatrix Aqq;
	DiagMatrix Apq;
	float detA;
	//std::vector<ModelOBJ::OBJVertex> intermediateSpeed;
	//std::vector<ModelOBJ::OBJVertex> intermediatePosition;

	float rawApqT[16];
	float ApqTApq[16];
	float rawApq[16];
	DiagMatrix ApqTApqSquared;
	float rawApqTApqSquared[16];
	VMatrix newMatR;

	
	//void calculateRotationAngles( VMatrix& matR, float* rotX, float* rotY, float* rotZ );
	double getTotalMass();
	void resetWeights();
	void updateDeepness( float yPosition );

	void calculateCenterOfMass( float* cm, std::vector<float>& pos );
	void calculateCenterOfMass( float* cm, std::vector<ModelOBJ::OBJVertex*>& pos );


	//void calculatingGoalPositions( float dt );
	// Id of the cluster
	int clusterId;

	// total number of vertices of the model
	int numberOfVertices;

	// current vertex buffer
	std::vector<ModelOBJ::OBJVertex*> m_vertexBuffer;

	// vertex speed
	std::vector<ModelOBJ::OBJVertex*> vertexSpeed;

	// intermediate coordinates
	std::vector<ModelOBJ::OBJVertex*> intermediatePosition;
	std::vector<ModelOBJ::OBJVertex*> intermediateSpeed;

	bool recalculateWeights;
	float deepness;

	//std::map< int, float > connections;
	std::map< int, Connection > connections;

	// original vertex buffer
	std::vector<float> originalVertexBuffer;

	// original and updated center of mass and
	float cm0[3];
	float cm[3];
	float previousCM[3];

	int relaxedUpdates;

	bool isSleeping;

	// pi and qi vectors
	std::vector<float> p;
	std::vector<float> q;

	// vertex individual weight and total mass
	//std::vector<double> originalWeight;
	//std::vector<double> weight;

	// Apq matrix raw data
};

#endif
